import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

    # 终端 A: Gazebo
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('lerobot_description'),
                'launch',
                'so101_gazebo.launch.py'
            ])
        )
    )

    # 终端 B (Part 1): 控制器
    controllers_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('lerobot_controller'),
                'launch',
                'so101_controller.launch.py'
            ])
        )
    )

    # 终端 B (Part 2): MoveIt
    # 注意: 自动为MoveIt设置 is_sim:=True
    # RViz (display) 将使用其默认值 (True)，这在仿真中是我们想要的
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('lerobot_moveit'),
                'launch',
                'so101_moveit.launch.py'
            ])
        ),
        launch_arguments={'is_sim': 'True'}.items()
    )

    # 终端 C: 手柄控制
    # 注意: 传递 use_sim:=true
    joystick_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare('joystick_alphadog_with_so101_servo'),
                'launch',
                'unified_control.launch.py'
            ])
        ),
        launch_arguments={'use_sim': 'true'}.items()
    )

    return LaunchDescription([
        gazebo_launch,
        controllers_launch,
        moveit_launch,
        joystick_launch
    ])